import csv


pathTum = '/home/ubuntu/user_space/dataset-room1_1024_16/mav0/imu0/data.csv'
pathTumGT = '/home/ubuntu/user_space/dataset-room1_1024_16/dso/gt_imu.csv'

def loadTumImu():
    file = open(pathTum)
    fileTum = csv.reader(file)

    tum_accex, tum_accey, tum_accez = [], [], []
    tum_angux, tum_anguy, tum_anguz = [], [], []
    time = []

    cnt = 1
    next(fileTum)
    for row in fileTum:
        tum_angux.append(float(row[1]))
        tum_anguy.append(float(row[2]))
        tum_anguz.append(float(row[3]))
        tum_accex.append(float(row[4]))
        tum_accey.append(float(row[5]))
        tum_accez.append(float(row[6]))
        time.append(float(row[0]))
        cnt += 1

### time haixu chuli cheng heshi de geshi zai fanhui
    return time, tum_accex, tum_accey, tum_accez, tum_angux, tum_anguy, tum_anguz

def loadTumGT():
    fileGT = csv.reader(open(pathTumGT))

    tum_px, tum_py, tum_pz = [], [], []
    tum_q0, tum_q1, tum_q2, tum_q3 = [], [], [], []

    next(fileGT)
    for row in fileGT:
        tum_px.append(float(row[1]))
        tum_py.append(float(row[2]))
        tum_pz.append(float(row[3]))
        tum_q0.append(float(row[4]))
        tum_q1.append(float(row[5]))
        tum_q2.append(float(row[6]))
        tum_q3.append(float(row[7]))

    return tum_px, tum_py, tum_pz, tum_q0, tum_q1, tum_q2, tum_q3



def getTime(t):

    t = int(t / (10 ** 6))
    print(t)
    t = t - 1520530308181

    return t

def loadTumGtInterp():
    fileGT = csv.reader(open(pathTumGT))

    time = []
    tum_px, tum_py, tum_pz = [], [], []
    tum_q0, tum_q1, tum_q2, tum_q3 = [], [], [], []

    next(fileGT)
    for row in fileGT:
        time.append(float(row[0]))
        tum_px.append(float(row[1]))
        tum_py.append(float(row[2]))
        tum_pz.append(float(row[3]))
        tum_q0.append(float(row[4]))
        tum_q1.append(float(row[5]))
        tum_q2.append(float(row[6]))
        tum_q3.append(float(row[7]))

    return time, tum_px, tum_py, tum_pz, tum_q0, tum_q1, tum_q2, tum_q3
